#include <stdio.h>
#include "EPuck.h"
#include <sstream>
#include <string>

#define pi 3.1415926535897932384626433832795

double modulus(double a, double b)
{
int result = static_cast<int>( a / b );
return a - static_cast<double>( result ) * b;
}

double angDiff(double ang1, double ang2) {
	double dif = modulus(((ang2+(2*pi)) - (ang1+(2*pi))),(2*pi));

	while( dif < -pi) dif+=2*pi;
	while( dif > pi) dif-=2*pi;

/*	if (ang1>ang2) {
		dif += 2*pi;
	}
	if (dif >= pi) {
		dif = -(dif - 2*pi);
	}
*/
	return dif;
}


/**
Test program for EPuck class.
Runs through functions in EPuck class and prints responses to standard output.
*/
int main(void)
{
	const int num_robots = 10; //how many robots to make
	EPuck* robots[num_robots];

	char robotNames[num_robots][10];//names are 10 chars long
	memset( robotNames, '\0', 10*num_robots );

	unsigned long maxAggTimer = 15;
	unsigned long aggTimers[num_robots] = {0};

	enum robotModeType {SEEK_MODE, GROUP_MODE};
	robotModeType robotModes[num_robots] = {GROUP_MODE};

	for (int i=0;i<num_robots;i++)//make the robots
	{
		// all of this ugliness just to get the name in a char*. dammit c++
		std::string robotNameString;
		std::stringstream tempStream;
		tempStream << "robot" << i;
		robotNameString = tempStream.str();
		robotNameString.copy( robotNames[i], 10 );

		std::cout <<"Creating "<<robotNames[i]<<", robot "<<i<<" of "<<num_robots<<", on port:"<<6665+i<<"\n";
		robots[i] = new EPuck(6665+i, robotNames[i] );
	}


	int i=0;
	while(true)
	{

		//find the centroid of the swarm
		double sumX = 0.0;
		double sumY = 0.0;
		double centX = 0.0;
		double centY = 0.0;

		for(int robotNum=0;robotNum<num_robots;robotNum++){
			sumX += robots[robotNum]->p2dProxy->GetXPos();
			sumY += robots[robotNum]->p2dProxy->GetYPos();
		}
		centX = sumX/num_robots;
		centY = sumY/num_robots;

//		centX = 2.0;
//		centY = 0.5;

		std::cout<<"swarm centre at: "<<centX<<", "<<centY<<"\n";

		double *ir;

		//Begin per-robot code
		for(int robotNum=0;robotNum<num_robots;robotNum++){

			aggTimers[robotNum]++;
			std::cout<<aggTimers[robotNum]<<"\n";

			// Collect IR data and calculate mean
			ir = robots[robotNum]->getIRReadings();
			double numSensors = 8.0;
			double irSum = 0;
			for(int irNum=0;irNum<numSensors;irNum++){
				irSum += ir[irNum];
			}
			double irMean = (irSum/numSensors);

			if (robotModes[robotNum] == GROUP_MODE) {
				double dX = centX-robots[robotNum]->p2dProxy->GetXPos();
				double dY = centY-robots[robotNum]->p2dProxy->GetYPos();

		//		std::cout<<"robot 0 yaw is: "<<robots[0]->p2dProxy->GetYaw()<<"\n";
		//		std::cout<<"robot 0 dir to centre:  "<<atan2(dY,dX)<<"\n";
		//		std::cout<<"robot angle difference:  "<<angDiff(robots[0]->p2dProxy->GetYaw(),atan2(dY,dX))<<"\n";

				double steerVal = ((angDiff(robots[robotNum]->p2dProxy->GetYaw(),atan2(dY,dX))/100.0));
				double speed = ((pi-abs(steerVal))/pi)*0.06;
				robots[robotNum]->setDifferentialMotors(speed-steerVal, speed+steerVal);



				if ((ir[0]+ir[1]+ir[6]+ir[7])<0.4) {
					aggTimers[robotNum] = 0; //we bumped something, reset the aggregation timer
					robotModes[robotNum] = SEEK_MODE; //and switch to seek mode
					if ((ir[0]+ir[1])<(ir[6]+ir[7])){
						robots[robotNum]->setMotors(0.0, 0.6);
					} else {
						robots[robotNum]->setMotors(0.0, -0.6);
					}
				}

			} else if (robotModes[robotNum] == SEEK_MODE) {
				bool seesLight = false;
				int noBlobs = robots[robotNum]->getNumberBlobs();
				if(noBlobs > 0) {
					for(i=0; i<noBlobs; i++) {
						Blob myblob;
						myblob = robots[robotNum]->getBlob(i);

//						std::cout<<(myblob.colour==0xff0000);
						if (myblob.colour == 0x00ff00) {
							seesLight = true;
							double steerVal = ((myblob.x - 320)/64000.0);
							robots[robotNum]->setDifferentialMotors(0.06+steerVal, 0.06-steerVal);
	//						robots[robotNum]->setDifferentialMotors(-0.006, -0.006);
						}
					}
				}
				if (!seesLight) {
					robots[robotNum]->setDifferentialMotors(0, 0.06);
				}

				if ((ir[0]+ir[1]+ir[6]+ir[7])<0.4) {
					std::cout<<"bumped after"<<aggTimers[robotNum]<<"\n";
					aggTimers[robotNum] = 0; //we bumped something, reset the aggregation timer
					if ((ir[0]+ir[1])<(ir[6]+ir[7])){
						robots[robotNum]->setMotors(-0.0, 0.6);
					} else {
						robots[robotNum]->setMotors(-0.0, -0.6);
					}
				}

				if (aggTimers[robotNum] > maxAggTimer) {
					robotModes[robotNum] = GROUP_MODE;
				}
	
			}
//		}
/*
		for(int robotNum=0;robotNum<num_robots;robotNum++){

/*
			int noBlobs = robots[robotNum]->getNumberBlobs();
			if(noBlobs > 0)
			{
				for(i=0; i<noBlobs; i++)
				{
					Blob myblob;
					myblob = robots[robotNum]->getBlob(i);

					std::cout<<(myblob.colour==0xff0000);
					if (myblob.colour == 0xff0000) {
						double steerVal = ((myblob.x - 320)/64000.0);
//						robots[robotNum]->setDifferentialMotors(0.06+steerVal, 0.06-steerVal);
						robots[robotNum]->setDifferentialMotors(-0.006, -0.006);
					} else {
						robots[robotNum]->setDifferentialMotors(0.06, 0.06);
					}
				}
			}

*/
/*

			ir = robots[robotNum]->getIRReadings();
			double numSensors = 8.0;
			double irSum = 0;
			for(int irNum=0;irNum<numSensors;irNum++){
				irSum += ir[irNum];
			}
			double irMean = (irSum/numSensors);


			if ((ir[0]+ir[1]+ir[6]+ir[7])<0.4) {
				robots[robotNum]->setMotors(0.0, -1);
//				if ((ir[0]+ir[1])<(ir[6]+ir[7])){
//					robots[robotNum]->setMotors(-0.02, 0.6);
//				} else {
//					robots[robotNum]->setMotors(-0.02, -0.6);
//				}
			} //else {
//				robots[i]->setMotors(0.06, 0);
/*
				int noBlobs = robots[robotNum]->getNumberBlobs();
				if(noBlobs > 0)
				{
					for(i=0; i<noBlobs; i++)
					{
						Blob myblob;
						myblob = robots[robotNum]->getBlob(i);
//						std::cout<<myblob.colour<<"\n";
						if (myblob.colour == 0xff0000) {
							double steerVal = ((myblob.x - 320)/64000.0);
							robots[robotNum]->setDifferentialMotors(0.06+steerVal, 0.06-steerVal);
						} else if (myblob.colour == 0x00ff00) {
							double steerVal = ((myblob.x - 320)/64000.0);
							robots[robotNum]->setDifferentialMotors(0.06+steerVal, 0.06-steerVal);
						}
					}
				} else {
					robots[robotNum]->setMotors(0, -0.3);
//					robots[robotNum]->setMotors(0.06, 0);
				}

			}


		}
//		printf("robot's IR readings are:\n");
//		printf("\t%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f\n", \
				ir[0], ir[1], ir[2], ir[3], ir[4], ir[5], ir[6], ir[7]);

*/

		}
		usleep(50);
	}
	
	return 0;
}









